/*
 * @Author: 姚潘涛
 * @Date: 2024-04-08 15:18:03
 * @LastEditors: 姚潘涛
 * @LastEditTime: 2024-11-12 16:42:29
 * @Description:
 *
 * Copyright (c) 2024 by pandaman, All Rights Reserved.
 */
#include "xict_cpp/utils/config.h"

namespace xict_cpp {
    namespace utils {
        bool LoadConfig(YAML::Node& config_yaml, const std::string& node) {
            if (!xict_common::utils::CheckYamlSubNode(config_yaml, node)) {
                AERROR_F("Failed to load node: {}", node.c_str());
                return false;
            }
            return true;
        }

        bool UpdateRemoteControllerPipelineParams(
            const YAML::Node& config_yaml,
            xict_cpp::core::pipeline::RemoteControllerPipelineParams&
                va_params) {
            bool ret = true;
            // 图像topic
            YAML::Node topic_config = config_yaml["topic"];
            if (topic_config) {
                if (topic_config["front_image_topic"]) {
                    va_params.stream.front_image_topic =
                        topic_config["front_image_topic"].as<std::string>();
                }
                if (topic_config["back_image_topic"]) {
                    va_params.stream.back_image_topic =
                        topic_config["back_image_topic"].as<std::string>();
                }
                if (topic_config["left_image_topic"]) {
                    va_params.stream.left_image_topic =
                        topic_config["left_image_topic"].as<std::string>();
                }
                if (topic_config["right_image_topic"]) {
                    va_params.stream.right_image_topic =
                        topic_config["right_image_topic"].as<std::string>();
                }

                // 图像识别topic
                if (topic_config["yolo_front"]) {
                    va_params.stream.yolo_front =
                        topic_config["yolo_front"].as<std::string>();
                }
                if (topic_config["yolo_back"]) {
                    va_params.stream.yolo_back =
                        topic_config["yolo_back"].as<std::string>();
                }
                if (topic_config["yolo_left"]) {
                    va_params.stream.yolo_left =
                        topic_config["yolo_left"].as<std::string>();
                }
                if (topic_config["yolo_right"]) {
                    va_params.stream.yolo_right =
                        topic_config["yolo_right"].as<std::string>();
                }
            }

            // 功能开关
            YAML::Node switcher_config = config_yaml["switcher"];
            if (switcher_config) {
                if (switcher_config["enable_stream"]) {
                    va_params.switcher.enable_stream =
                        switcher_config["enable_stream"].as<bool>();
                }
                if (switcher_config["enable_can"]) {
                    va_params.switcher.enable_can =
                        switcher_config["enable_can"].as<bool>();
                }
                if (switcher_config["enable_heartbeat"]) {
                    va_params.switcher.enable_heartbeat =
                        switcher_config["enable_heartbeat"].as<bool>();
                }
                if (switcher_config["enable_draw_trajectory"]) {
                    va_params.switcher.enable_draw_trajectory =
                        switcher_config["enable_draw_trajectory"].as<bool>();
                }
                if (switcher_config["enable_ui_thread"]) {
                    va_params.switcher.enable_ui_thread =
                        switcher_config["enable_ui_thread"].as<bool>();
                }
                if (switcher_config["enable_unity_thread"]) {
                    va_params.switcher.enable_unity_thread =
                        switcher_config["enable_unity_thread"].as<bool>();
                }
            }

            // rtsp推流
            YAML::Node rtsp_config = config_yaml["rtsp"];
            if (rtsp_config) {
                if (rtsp_config["rtsp_enable"]) {
                    va_params.stream.rtsp_enable =
                        rtsp_config["rtsp_enable"].as<bool>();
                }
                if (rtsp_config["rtsp_url"]) {
                    va_params.stream.rtsp_url =
                        rtsp_config["rtsp_url"].as<std::string>();
                }
                if (rtsp_config["rtsp_bitrate"]) {
                    va_params.stream.rtsp_bitrate =
                        rtsp_config["rtsp_bitrate"].as<int>();
                }
                if (rtsp_config["rtsp_width"]) {
                    va_params.stream.rtsp_width =
                        rtsp_config["rtsp_width"].as<int>();
                }
                if (rtsp_config["rtsp_height"]) {
                    va_params.stream.rtsp_height =
                        rtsp_config["rtsp_height"].as<int>();
                }
                if (rtsp_config["rtsp_fps"]) {
                    va_params.stream.rtsp_fps =
                        rtsp_config["rtsp_fps"].as<int>();
                }
                if (rtsp_config["rtsp_server_exe"]) {
                    va_params.stream.rtsp_server_exe =
                        rtsp_config["rtsp_server_exe"].as<std::string>();
                }
                if (rtsp_config["rtsp_server_config"]) {
                    va_params.stream.rtsp_server_config =
                        rtsp_config["rtsp_server_config"].as<std::string>();
                }
            }

            // 检测模型配置参数
            YAML::Node model_config = config_yaml["model"];
            if (model_config) {
                if (model_config["image_width"]) {
                    va_params.stream.image_width =
                        model_config["image_width"].as<int>();
                }
                if (model_config["image_height"]) {
                    va_params.stream.image_height =
                        model_config["image_height"].as<int>();
                }
                if (model_config["model_flag"]) {
                    va_params.stream.model_flag =
                        model_config["model_flag"].as<bool>();
                }
                if (va_params.stream.model_flag && model_config["model_path"]) {
                    std::string model_path =
                        model_config["model_path"].as<std::string>();
                    if (!model_path.empty()) {
                        va_params.stream.model_path = model_path;
                    } else {
                        AERROR_F("model path is empty!");
                        ret = false;
                    }
                }
                if (model_config["ignore_region_flag"]) {
                    va_params.stream.ignore_region_flag =
                        model_config["ignore_region_flag"].as<bool>();
                }
                if (model_config["mask_show_flag"]) {
                    va_params.stream.mask_show_flag =
                        model_config["mask_show_flag"].as<bool>();
                }
                if (model_config["camera_ignore_regions"]) {
                    va_params.stream.camera_ignore_regions.clear();

                    for (const auto& camera_it :
                         model_config["camera_ignore_regions"]) {
                        const std::string& camera_name =
                            camera_it.first.as<std::string>();
                        std::vector<std::vector<cv::Point>> ignore_regions;

                        // 遍历该相机的所有区域
                        for (const auto& region_node : camera_it.second) {
                            if (!region_node["region"]) {
                                AWARN_F("Invalid region format for camera: {}",
                                        camera_name);
                                continue;
                            }

                            std::vector<cv::Point> region;
                            // 读取该区域的所有点
                            for (const auto& point : region_node["region"]) {
                                try {
                                    if (!point.IsSequence() ||
                                        point.size() != 2) {
                                        AWARN_F("Invalid point format in "
                                                "camera: {}",
                                                camera_name);
                                        continue;
                                    }
                                    int x = point[0].as<int>();
                                    int y = point[1].as<int>();
                                    region.emplace_back(x, y);
                                } catch (const YAML::Exception& e) {
                                    AERROR_F(
                                        "Error parsing point for camera {}: {}",
                                        camera_name, e.what());
                                    continue;
                                }
                            }

                            if (!region.empty()) {
                                ignore_regions.push_back(region);
                            }
                        }

                        // 存储该相机的忽略区域
                        va_params.stream.camera_ignore_regions[camera_name] =
                            ignore_regions;

                        // 创建该相机的mask
                        cv::Mat camera_mask = cv::Mat::zeros(
                            va_params.stream.image_height,
                            va_params.stream.image_width, CV_8UC1);
                        for (const auto& region : ignore_regions) {
                            cv::fillPoly(
                                camera_mask,
                                std::vector<std::vector<cv::Point>> {region},
                                cv::Scalar(255));
                        }
                        va_params.stream.camera_masks[camera_name] =
                            camera_mask;

                        ADEBUG_F(
                            "Initialized mask for camera: {} with {} regions",
                            camera_name, ignore_regions.size());
                    }
                }
            }

            // 驾舱相关配置参数
            YAML::Node cockpit_config = config_yaml["cockpit"];
            if (cockpit_config) {
                if (cockpit_config["canet_ip"]) {
                    va_params.canet.canet_ip =
                        cockpit_config["canet_ip"].as<std::string>();
                }
                if (cockpit_config["canet_port"]) {
                    va_params.canet.canet_port =
                        cockpit_config["canet_port"].as<int>();
                }
                if (cockpit_config["canet_factory"]) {
                    va_params.canet.vehicle_type =
                        cockpit_config["canet_factory"].as<std::string>();
                }
                if (cockpit_config["vehicle_type"]) {
                    va_params.canet.vehicle_type =
                        cockpit_config["vehicle_type"].as<std::string>();
                }
                if (cockpit_config["have_sensation_platform"]) {
                    va_params.canet.have_sensation_platform =
                        cockpit_config["have_sensation_platform"].as<bool>();
                }
                if (cockpit_config["sensation_platform_ip"]) {
                    va_params.canet.sensation_platform_ip =
                        cockpit_config["sensation_platform_ip"]
                            .as<std::string>();
                }
                if (cockpit_config["sensation_platform_port"]) {
                    va_params.canet.sensation_platform_port =
                        cockpit_config["sensation_platform_port"].as<int>();
                }
                if (cockpit_config["sensation_platform_range_x"]) {
                    va_params.canet.sensation_platform_range_x =
                        cockpit_config["sensation_platform_range_x"].as<int>();
                }
                if (cockpit_config["sensation_platform_range_y"]) {
                    va_params.canet.sensation_platform_range_y =
                        cockpit_config["sensation_platform_range_y"].as<int>();
                }
                if (cockpit_config["sensation_platform_range_z"]) {
                    va_params.canet.sensation_platform_range_z =
                        cockpit_config["sensation_platform_range_z"].as<int>();
                }
                if (cockpit_config["use_selfmade_handle"]) {
                    va_params.canet.use_selfmade_handle =
                        cockpit_config["use_selfmade_handle"].as<bool>();
                }
                if (cockpit_config["msg_framerate"]) {
                    va_params.canet.msg_framerate =
                        cockpit_config["msg_framerate"].as<int>();
                }
            }

            // 心跳包发送配置参数
            YAML::Node heartbeat_config = config_yaml["heartbeat"];
            if (heartbeat_config) {
                if (heartbeat_config["enable"]) {
                    va_params.heartbeat.enable =
                        heartbeat_config["enable"].as<bool>();
                }
                if (heartbeat_config["frequency"]) {
                    va_params.heartbeat.frequency =
                        heartbeat_config["frequency"].as<float>();
                }
                if (heartbeat_config["ip"]) {
                    va_params.heartbeat.ip =
                        heartbeat_config["ip"].as<std::string>();
                }
                if (heartbeat_config["port"]) {
                    va_params.heartbeat.port =
                        heartbeat_config["port"].as<int>();
                }
                if (heartbeat_config["cmd_heartbeat"]) {
                    va_params.heartbeat.cmd_heartbeat =
                        heartbeat_config["cmd_heartbeat"].as<std::string>();
                }
                if (heartbeat_config["is_log"]) {
                    va_params.heartbeat.is_log =
                        heartbeat_config["is_log"].as<bool>();
                }
            }

            // 脚本路径
            YAML::Node script_path_config = config_yaml["script_path"];
            if (script_path_config) {
                if (script_path_config["unity_start_script"]) {
                    va_params.scripts_path.unity_start_script =
                        script_path_config["unity_start_script"]
                            .as<std::string>();
                }
                if (script_path_config["unity_end_script"]) {
                    va_params.scripts_path.unity_end_script =
                        script_path_config["unity_end_script"]
                            .as<std::string>();
                }
                if (script_path_config["ui_app_path"]) {
                    va_params.scripts_path.ui_app_path =
                        script_path_config["ui_app_path"].as<std::string>();
                }
            }

            // 绘制多组车道线配置参数
            YAML::Node trajectories_node = config_yaml["trajectory"];
            if (trajectories_node) {
                // 遍历每个 trajectory 节点，创建多组参数
                for (std::size_t i = 0; i < trajectories_node.size(); ++i) {
                    xict_cpp::core::stage::DrawTrajectoryStageParams params;
                    // 解析单个 trajectory 节点中的所有参数
                    YAML::Node drawtrajectory_config = trajectories_node[i];

                    // 轨迹形式
                    if (drawtrajectory_config["trajectory_type"]) {
                        params.trajectory_type =
                            drawtrajectory_config["trajectory_type"]
                                .as<std::string>();
                    }
                    // 轨迹图像发布的topic
                    if (drawtrajectory_config["pub_topic"]) {
                        params.pub_topic = drawtrajectory_config["pub_topic"]
                                               .as<std::string>();
                    }
                    // 使用轨迹图像的topic发布
                    if (drawtrajectory_config["enable_pub"]) {
                        params.enable_pub =
                            drawtrajectory_config["enable_pub"].as<bool>();
                    }
                    // 旋转向量
                    if (drawtrajectory_config["rotation_vector"]) {
                        std::vector<double> rotation_vector_data =
                            drawtrajectory_config["rotation_vector"]
                                .as<std::vector<double>>();
                        params.rotation_vector = cv::Vec3d(
                            rotation_vector_data[0], rotation_vector_data[1],
                            rotation_vector_data[2]);
                    }
                    // 平移向量
                    if (drawtrajectory_config["translation_vector"]) {
                        std::vector<double> translation_vector_data =
                            drawtrajectory_config["translation_vector"]
                                .as<std::vector<double>>();
                        params.translation_vector =
                            cv::Vec3d(translation_vector_data[0],
                                      translation_vector_data[1],
                                      translation_vector_data[2]);
                    }
                    // 相机内参矩阵
                    if (drawtrajectory_config["camera_matrix"]) {
                        std::vector<double> camera_matrix_data =
                            drawtrajectory_config["camera_matrix"]
                                .as<std::vector<double>>();
                        params.camera_matrix =
                            cv::Mat(3, 3, CV_64FC1, camera_matrix_data.data())
                                .clone();
                    }

                    // 相机畸变参数矩阵
                    if (drawtrajectory_config["dist_coeffs"]) {
                        std::vector<double> dist_coeffs_data =
                            drawtrajectory_config["dist_coeffs"]
                                .as<std::vector<double>>();
                        params.dist_coeffs =
                            cv::Mat(5, 1, CV_64FC1, dist_coeffs_data.data())
                                .clone();
                    }
                    // 图像宽度
                    if (drawtrajectory_config["image_width"]) {
                        params.image_width =
                            drawtrajectory_config["image_width"].as<int>();
                    }
                    // 图像高度
                    if (drawtrajectory_config["image_height"]) {
                        params.image_height =
                            drawtrajectory_config["image_height"].as<int>();
                    }
                    // 图像类型
                    if (drawtrajectory_config["image_type"]) {
                        params.image_type =
                            drawtrajectory_config["image_type"].as<int>();
                    }
                    // 拟合点数
                    if (drawtrajectory_config["num_points"]) {
                        params.num_points =
                            drawtrajectory_config["num_points"].as<int>();
                    }
                    // 线宽
                    if (drawtrajectory_config["line_thickness"]) {
                        params.line_thickness =
                            drawtrajectory_config["line_thickness"].as<int>();
                    }

                    // 线颜色
                    if (drawtrajectory_config["line_color"]) {
                        params.line_color = drawtrajectory_config["line_color"]
                                                .as<std::vector<int>>();
                    }

                    // 荧光线宽度
                    if (drawtrajectory_config["glow_line_width"]) {
                        params.glow_line_width =
                            drawtrajectory_config["glow_line_width"].as<int>();
                    }

                    // 荧光色渐变比率
                    if (drawtrajectory_config["glow_line_ratio"]) {
                        params.glow_line_ratio =
                            drawtrajectory_config["glow_line_ratio"]
                                .as<double>();
                    }

                    // 荧光色线宽度
                    if (drawtrajectory_config["glow_line_thickness"]) {
                        params.glow_line_thickness =
                            drawtrajectory_config["glow_line_thickness"]
                                .as<int>();
                    }

                    // 5m位置点
                    if (drawtrajectory_config["position_5m"]) {
                        params.position_5m =
                            drawtrajectory_config["position_5m"].as<int>();
                    }

                    // 10m位置点
                    if (drawtrajectory_config["position_10m"]) {
                        params.position_10m =
                            drawtrajectory_config["position_10m"].as<int>();
                    }

                    // 15m位置点
                    if (drawtrajectory_config["position_15m"]) {
                        params.position_15m =
                            drawtrajectory_config["position_15m"].as<int>();
                    }

                    // 距离标识绘制高度x偏移
                    if (drawtrajectory_config["text_position_x"]) {
                        params.text_position_x =
                            drawtrajectory_config["text_position_x"].as<int>();
                    }

                    // 距离标识绘制高度y偏移
                    if (drawtrajectory_config["text_position_y"]) {
                        params.text_position_y =
                            drawtrajectory_config["text_position_y"].as<int>();
                    }

                    // 距离标识字体内颜色
                    if (drawtrajectory_config["font_color_inner"]) {
                        params.font_color_inner =
                            drawtrajectory_config["font_color_inner"]
                                .as<std::vector<int>>();
                    }

                    // 距离标识字体外颜色
                    if (drawtrajectory_config["font_color_outer"]) {
                        params.font_color_outer =
                            drawtrajectory_config["font_color_outer"]
                                .as<std::vector<int>>();
                    }

                    // 距离标识字体荧光宽度
                    if (drawtrajectory_config["glow_font_width"]) {
                        params.glow_font_width =
                            drawtrajectory_config["glow_font_width"].as<int>();
                    }

                    // 距离标识字体大小
                    if (drawtrajectory_config["font_size"]) {
                        params.font_size =
                            drawtrajectory_config["font_size"].as<double>();
                    }

                    // 距离标识字体粗细
                    if (drawtrajectory_config["font_thickness"]) {
                        params.font_thickness =
                            drawtrajectory_config["font_thickness"]
                                .as<double>();
                    }

                    // 箭头流动速度
                    if (drawtrajectory_config["arrow_speed"]) {
                        params.arrow_speed =
                            drawtrajectory_config["arrow_speed"].as<double>();
                    }

                    // 箭头粗细
                    if (drawtrajectory_config["arrow_thickness"]) {
                        params.arrow_thickness =
                            drawtrajectory_config["arrow_thickness"].as<int>();
                    }

                    // 箭头长度
                    if (drawtrajectory_config["arrow_length"]) {
                        params.arrow_length =
                            drawtrajectory_config["arrow_length"].as<int>();
                    }

                    // 箭头角度
                    if (drawtrajectory_config["arrow_angle"]) {
                        params.arrow_angle =
                            drawtrajectory_config["arrow_angle"].as<double>();
                    }

                    // 箭头颜色
                    if (drawtrajectory_config["arrow_color"]) {
                        params.arrow_color =
                            drawtrajectory_config["arrow_color"]
                                .as<std::vector<int>>();
                    }

                    // 箭头荧光宽度
                    if (drawtrajectory_config["glow_arrow_width"]) {
                        params.glow_arrow_width =
                            drawtrajectory_config["glow_arrow_width"].as<int>();
                    }

                    // 箭头荧光比率
                    if (drawtrajectory_config["glow_arrow_ratio"]) {
                        params.glow_arrow_ratio =
                            drawtrajectory_config["glow_arrow_ratio"]
                                .as<double>();
                    }

                    // 箭头荧光线宽度
                    if (drawtrajectory_config["glow_arrow_thickness"]) {
                        params.glow_arrow_thickness =
                            drawtrajectory_config["glow_arrow_thickness"]
                                .as<int>();
                    }

                    // 多箭头绘制间距
                    if (drawtrajectory_config["arrow_interval"]) {
                        params.arrow_interval =
                            drawtrajectory_config["arrow_interval"].as<int>();
                    }

                    // 线条分段颜色
                    if (drawtrajectory_config["multi_colors"]) {
                        params.multi_colors =
                            drawtrajectory_config["multi_colors"]
                                .as<std::vector<std::string>>();
                    }

                    // 线段过渡比率
                    if (drawtrajectory_config["multi_color_alpha_ratio"]) {
                        params.multi_color_alpha_ratio =
                            drawtrajectory_config["multi_color_alpha_ratio"]
                                .as<double>();
                    }

                    // 线段颜色透明度
                    if (drawtrajectory_config["alpha_channel_transparency"]) {
                        params.alpha_channel_transparency =
                            drawtrajectory_config["alpha_channel_transparency"]
                                .as<int>();
                    }

                    // 待拟合的贝塞尔曲线控制点
                    if (drawtrajectory_config["curves_points"]) {
                        const YAML::Node& curves_points_node =
                            drawtrajectory_config["curves_points"];
                        for (std::size_t j = 0; j < curves_points_node.size();
                             ++j) {
                            xict_cpp::core::stage::BezierCurveParams curve;

                            if (curves_points_node[j]["start_point"]) {
                                std::vector<double> sp =
                                    curves_points_node[j]["start_point"]
                                        .as<std::vector<double>>();
                                curve.start_point =
                                    cv::Point3d(sp[0], sp[1], sp[2]);
                            }

                            if (curves_points_node[j]["control_point"]) {
                                std::vector<double> cp =
                                    curves_points_node[j]["control_point"]
                                        .as<std::vector<double>>();
                                curve.control_point =
                                    cv::Point3d(cp[0], cp[1], cp[2]);
                            }

                            if (curves_points_node[j]["end_point"]) {
                                std::vector<double> ep =
                                    curves_points_node[j]["end_point"]
                                        .as<std::vector<double>>();
                                curve.end_point =
                                    cv::Point3d(ep[0], ep[1], ep[2]);
                            }

                            params.curves_points.push_back(curve);
                        }
                    }
                    // 线长度
                    if (drawtrajectory_config["line_length"]) {
                        params.line_length =
                            drawtrajectory_config["line_length"].as<double>();
                    }
                    // 车型
                    if (drawtrajectory_config["vehicle_type"]) {
                        params.vehicle_type =
                            drawtrajectory_config["vehicle_type"]
                                .as<std::string>();
                    }

                    va_params.trajectories_params_list.emplace_back(params);
                }
            }
            return ret;
        }
    }   // namespace utils
}   // namespace xict_cpp
